
#include "mb_comm.h"

/**
 * @brief 位操作
 * @param value        值
 * @param pReg         指针,寄存器
 * @param StartAddress 开始的地址 
 * @ret   MB_OK 成功 MB_ERROR 错误
*/
MB_StatusTypeDef MB_BitSetReset(uint16_t value, uint8_t *pReg, uint16_t StartAddress)
{
  
	uint16_t byteoffset = StartAddress / 8;
	uint8_t  bitoffset = StartAddress % 8;

  uint8_t V = pReg[byteoffset];	
	
	if(value > 0)
	{
		pReg[byteoffset] = MB_BIT_SET(V, bitoffset);
	}
	else
	{
		pReg[byteoffset] = MB_BIT_RESET(V, bitoffset);
	}

  return MB_OK;   
}

/**
 * @brief 位数组操作
 * @param pSrc         源数据
 * @param pReg         指针,寄存器
 * @param StartAddress 位开始的地址 
 * @param Quantity     位寄存器数量
 * @ret   MB_OK 成功 MB_ERROR 错误
*/
MB_StatusTypeDef MB_BitArraySetReset(uint8_t *pSrc, uint8_t *pReg, uint16_t StartAddress, uint16_t Quantity)
{
  int i;
	int Srcbyteoffset;
	int Srcbitoffset;
	
	int Regbyteoffset;
	int Regbitoffset;
	
	for(i=0; i<Quantity; i++)
	{
		Srcbyteoffset = i / 8;
		Srcbitoffset  = i % 8;
		
		Regbyteoffset = (StartAddress + i) / 8;
		Regbitoffset  = (StartAddress + i) % 8;
		
		if((pSrc[Srcbyteoffset] >> Srcbitoffset) & 0x01)
		{
			pReg[Regbyteoffset] = MB_BIT_SET(pReg[Regbyteoffset], Regbitoffset);
		} else {
			pReg[Regbyteoffset] = MB_BIT_RESET(pReg[Regbyteoffset], Regbitoffset);
		}
	}	
  return MB_OK;   
}

/**
 * @brief 位数组转字节数据, 读数据时,可能会从一个字节的中间读取,比如第1,2...7
 * @param hslave       从站指针
 * @param StartAddress 位开始的地址,非字节序号
 * @param Quantity     位数量
 * @param pDst         写入数据的位置
 * @param pDstLen      写入数据的长度
 * @ret   MB_OK 成功 MB_ERROR 错误
*/
MB_StatusTypeDef MB_BitArrayToByteArray(
    uint8_t  *pSrc, 
    uint16_t  StartAddress, 
    uint16_t  Quantity, 
    uint8_t  *pDst,
    uint16_t *pDstLen
    )
{
    uint16_t rspLen = 0;
    int i = 0;
    int j = 0;

    if(Quantity > 0)
    {
        rspLen = (Quantity - 1) / 8 + 1;

        for(i=0; i<rspLen; i++)
        {
            for(j=0; j<8; j++){
                if((i * 8 + j) < Quantity)
                {
                    pDst[ StartAddress + i] = pDst[ StartAddress + i] | (((uint8_t)pSrc[i * 8 + j]) << j);
                }
            }
        }
        *pDstLen = rspLen;  //返回写入的长度
        return MB_OK;
    } else {
        return MB_ERROR;
    }
}

/**
 * @brief 字节数据转位数组, 写数据时,可能会从一个字节的中间开始写,比如第1,2...7
 * @param hslave       从站指针
 * @param StartAddress 位开始的地址,非字节序号
 * @param Quantity     位数量
 * @param pDst         写入数据的位置
 * @param pDstLen      写入数据的长度
 * @ret   MB_OK 成功 MB_ERROR 错误
*/
MB_StatusTypeDef MB_ByteArrayToBitArray(
    uint8_t  *pSrc, 
    uint16_t  StartAddress, 
    uint16_t  Quantity, 
    uint8_t  *pDst,
    uint16_t *pDstLen
    )
{
    uint16_t rspLen = 0;
    uint8_t * p = &pDst[StartAddress];
    int i = 0;
    int j = 0;

    if(Quantity > 0)
    {
        rspLen = (Quantity - 1) / 8 + 1;

        for(i=0; i<rspLen; i++)
        {
            for(j=0; j<8; j++){
                if((i * 8 + j) < Quantity)
                {
                    if(((pSrc[i * 8 + j] >> j) & 0x01) == 1)
                    {
                         p[i] = pDst[i] | (1<< j);
                    } else {
                        p[i] = pDst[i] & (~(1<< j));
                    }
                }
            }
        }
        *pDstLen = rspLen;  //返回写入的长度
        return MB_OK;
    } else {
        return MB_ERROR;
    }
}

/**
 * @brief 字节数组复制, 复制到寄存器
 * @param pSrc            数据源
 * @param RegStartAddress 寄存器的地址,字节序号
 * @param Quantity        寄存器数量
 * @param pReg            指针,寄存器
 * @param pDstLen         写入数据的长度
 * @ret   MB_OK 成功 MB_ERROR 错误
*/
MB_StatusTypeDef MB_ByteArrayCopyTo(
    uint8_t  *pSrc, 
    uint16_t  RegStartAddress, 
    uint16_t  Quantity, 
    uint8_t  *pReg,
    uint16_t *pDstLen
    )
{
    uint16_t rspLen = 0;
    int i = 0;

    if(Quantity > 0)
    {
        rspLen = Quantity * 2;

        for(i=0; i<rspLen; i++)
        {
            pReg[RegStartAddress + i] = pSrc[i];
        }
        *pDstLen = rspLen;
        return MB_OK;
    } else {
        return MB_ERROR;
    }
}

/**
 * @brief 字节数组复制,从寄存器复制出来
 * @param pReg            指针,寄存器
 * @param RegStartAddress 寄存器开始的地址,字节序号
 * @param Quantity        寄存器数量
 * @param pDst            指针,写入数据的位置
 * @param pDstLen         指针,写入数据的长度
 * @ret   MB_OK 成功 MB_ERROR 错误
*/
MB_StatusTypeDef MB_ByteArrayCopyFrom(
    uint8_t  *pReg, 
    uint16_t  RegStartAddress, 
    uint16_t  Quantity, 
    uint8_t  *pDst,
    uint16_t *pDstLen
    )
{
    uint16_t rspLen = 0;
    int i = 0;

    if(Quantity > 0)
    {
        rspLen = Quantity * 2;

        for(i=0; i<rspLen; i++)
        {
            pDst[i] = pReg[RegStartAddress + i];
        }
        *pDstLen = rspLen;
        return MB_OK;
    } else {
        return MB_ERROR;
    }
}

